/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Robo;

import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.camera.AxisCamera;

/**
 *
 * @author Jay
 */
public class cameraPanTilt extends RoboTask{

	// Open Servo References
	// Remember: DS #2
	Servo cameraPan = new Servo(6,1);
	Servo cameraTilt = new Servo(6,2);

	// stores the current camera mode
	// valid options: "manual" (no motion), "elevator" (tracking), "minibot"
	String cameraMode = "elevator";

	// Keyframes for pan,tilt positions
	// 1 keyframe for each different "keyframed" arm/elevator position
	static final double[] panKeyFrames = {0.078, 0.083, 0.096, 0.284, 0.284, 0.350, 0.350};
	static final double[] tiltKeyFrames = {0.4265, 0.621, 0.74, 1.0, 1.0, 1.0, 1.0};
        double localCurrentHeight = 58;
        double localCurrentAngle = 0;

        int key = 0; // Identifies at the keyframes to look at (key-1 to key) for interpolation
        static int maxKeys = 7; // The total number of possible keyframes. Used to prevent infinite loops
        private double ratio = 0.5; // the current ratio used to interpolate between keyframes

        // variables for manual camera control
        private double pan = 0.0;
        private double tilt = 0.5;
        private boolean change = false;
        private double dc = 0.0005; //camera sensitivity

        // buttons for manual camera control
        private boolean panL = false;
        private boolean panR = false;
        private boolean tiltU= false;
        private boolean tiltD= false;

	void setMode(String newMode){
		cameraMode = newMode;
	}

	// Determine the linear "interpolation ratio" between two keyframes
	private double interpolate_ratio(double current, int n, double[] data){
            if (data[n] - data[n-1] == 0){
                return 0.5;
            }
            else{
                return ((current - data[n-1]) / (data[n] - data[n-1]));
            }
        }

	// Start, turn the camera straight forwards
	public void start(){
		super.start();
		cameraPan.set(0.0);
		cameraTilt.set(0.5);

	}

	// main operation
	public void handle(){
		super.handle();

                // collect height, angle data from manipulator function
                localCurrentHeight = Robo.RoboManipulator.robot.manipulator.currentHeight;
                localCurrentAngle = Robo.RoboManipulator.robot.manipulator.currentAngle;

                // get manual control references
                panL = robot.driverStation.rightDriveStick.getRawButton(6);
                panR = robot.driverStation.rightDriveStick.getRawButton(7);
                tiltU = robot.driverStation.rightDriveStick.getRawButton(11);
                tiltD = robot.driverStation.rightDriveStick.getRawButton(10);

                // set the mode to manual if necessary
                if (panL || panR || tiltU || tiltD ){
                    setMode("manual");
                }

		// if manual mode
		if ( cameraMode.equals("manual") ){
                    // get new joystick input (tapping enabled)

                    change = false;
                    //pan left
                    if (panL){
                        // increment value
                        if (pan >= dc){
                            pan -= dc;
                        }
                        change = true;
                    }
                    //pan right
                    if (panR){
                        // increment value
                        if (pan < 1.0){
                            pan += dc;
                        }
                        change = true;
                    }
                    //tilt up
                    if (tiltU){
                        // increment value
                        if (tilt < 1.0){
                            tilt += dc;
                        }
                        change = true;
                    }
                    //tilt down
                    if (tiltD){
                        // increment value
                        if (tilt >= dc){
                            tilt -= dc;
                        }
                        change = true;
                    }

                    // if all buttons are false, tap = false (DEBUGGING CODE)
                    /*if (change == true){
                        System.out.println("Pan, Tilt");
                        System.out.println(pan);
                        System.out.println(tilt);
                    }*/

                    // set camera to new values
                    cameraPan.set(pan);
                    cameraTilt.set(tilt);
		}

                // elevator mode
                if ( cameraMode.equals("elevator") ){
 
                    // Identify the relevant keyframes
                    if ( (localCurrentHeight+58.0) < 60 ){
                        for ( key = 1; key < maxKeys; key++){
                            if (localCurrentAngle >= Robo.RoboManipulator.robot.manipulator.pegAngles[key]){
                                ratio = interpolate_ratio(localCurrentAngle, key, Robo.RoboManipulator.robot.manipulator.pegAngles);
                                break;
                            }
                        }
                    }
                    else{
                        // use height to pick keyframes
                        for (key = 1; key < maxKeys; key++){
                            if ((localCurrentHeight+58.0) < Robo.RoboManipulator.robot.manipulator.pegHeights[key]){
                                ratio = interpolate_ratio(localCurrentHeight+58.0, key, Robo.RoboManipulator.robot.manipulator.pegHeights);
                                break;
                            }
                        }
                    }

                    // error trap (mostly for uncalibration errors)
                    if (key >= maxKeys){
                        key = 1;
                        ratio = 0.5;
                    }

                    // finish interpolation, apply to servos
                    cameraPan.set((panKeyFrames[key]-panKeyFrames[key-1])*ratio + panKeyFrames[key-1]);
                    cameraTilt.set((tiltKeyFrames[key] - tiltKeyFrames[key-1])*ratio + tiltKeyFrames[key-1]);
                }

		// if minibot mode
		if ( cameraMode.equals("minibot") ){
				// attempt to look at minibot
				cameraPan.set(1.0);
				cameraTilt.set(0.5);
		}
	}
}